#include <iostream>
#include <string.h>

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <semaphore.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>


#include "constants.h"
#include "other_functions.h"

#include "view.h"
#include "model.h"

#include "SMATModel.hh"
#include "display.hh"
#include "utils.hh"




void CView2D::RANSAC_results(IplImage *img)
{
    reproject2();
    inliers = GetInliersError(mask_show, maskoutliers, &error);
    // reproject2 just done at the end of RANSAC_posit
    //reproject2();
    //cvCmpS(maskout, 0, maskout, CV_CMP_EQ);//invert the mask
    //cvAnd(maskoutliers, mask_show, maskout);
    if(!TRACK)//inliers > MIN_INLIERS2)
	smat->remove_last_added(maskoutliers);
		
    //update the value of the outlier points with the reprojected value
    cvRepeat(maskoutliers, maskout2d);
    cvCopy(points2d_reproject, points2d, maskout2d);

    if (TRACK)
	update_track(img);

    //qsetmask_show();

}

int CView2D::RANSAC_posit(ransac_params_t * ransac_global_params)
{
    //copy the old vector
    pose_old = (*this);

    int inliersi;
    float errori;
    int i, p, bestfit, j;
    float besterror;
    int iter;
    struct ransac_seq * rseq;
    int index[100];
    
    bestfit = 0;
    besterror = 1000000.0;

    int npoints_mask = cvCountNonZero(mask_show);

    if (npoints_mask < ransac_global_params->minpoints)
    {
	printf("No se aplica RANSAC. Pocos puntos .---------------\n");
	cvWaitKey(0);
	if (npoints_mask < ransac_global_params->minpoints-1)
	{
	    printf ("Pocos puntos. No se puede ejecutar POSIT\n");
	    return 0;
	}
	else
	{
	    //no hay ransac. Se aplica posit con todos los puntos
	    cvCopy(mask_show, maskout);
	}
    }
    else
    {
	// se escoge la secuencia de datos adecuada segun el numero de puntos disponibles
	rseq = &ransac_global_params->pseqs[npoints_mask-
					    ransac_global_params->minpoints];
	//la secuencia supone que todos los puntos disponibles estan juntos.
	//como ya hay un mask_show que hace que lo npoints_mask disponibles no esten consicutivos,
	//hay que crear un subindice para encontrarlos
	for (i=0, j=0; j<npoints_mask; i++)
	    if (cvGetReal1D(mask_show, i))
		index[j++] = i;

	//iteraciones de RANSAC
	int total_iter;
	if ( ITERACIONES_RANSAC > rseq->head.nseqs)
	    total_iter = rseq->head.nseqs;
	else
	    total_iter = ITERACIONES_RANSAC;;
	for (iter=0; iter<total_iter; iter++)
	{
	    cvZero(masktemp);
	    //recorre la secuencia ITER para ir sacando los puntos a usar
	    for (i=0; i<rseq->head.n; i++)
	    {
		p = rseq->seq_data[iter*rseq->head.n+ i];
		// el punto se toma del subindice, creado segun maskin
		p = index[p];
		//se marca el punto correspondiente para usarlo en el POSIT
		cvSetReal1D(masktemp, p, 1);
	    }
	    
	    //se llama al posit con los puntos seleccionados, 
	    // para obtener un primer R, T
	    calcPose(masktemp);
	    if (distanceR(pose_old) > MAX_DIST_R || 
		distanceT2(pose_old) > MAX_DIST_T*MAX_DIST_T)
		;//continue;
	
	    // se reproyectan todos los puntos para calcular inliers y outliers
	    reproject2();
	    inliersi = GetInliersError(mask_show, maskoutliers, &errori);

	    if (inliersi > bestfit || (inliersi == bestfit && errori < besterror))
	    {
		bestfit = inliersi;
		besterror = errori;
		cvCopy(masktemp, maskout); //inliers con los que se calculó la pose
		//cvNot(maskoutliers, maskout); //todos los inliers del modelo ganador
	    }
	} //bucle RANSAC
    } //if(enough points for ransac)

    error = besterror/bestfit;
    return inliers = bestfit;
}

int CView2D::GetInliersError (CvMat * maskin, CvMat *v_outliers, float * errorint)
{
    int inliersint;

    // se calculan los inliers
    inliersint = 0;
    cvZero(v_outliers);

    //calcula el error de cada punto con respecto a la reproyección
    //calculo de la distancia euclidea
    cvSub(points2d, points2d_reproject, pointstemp); 
    cvMul(pointstemp, pointstemp, pointstemp);
    cvReduce(pointstemp, dist1D, 1, CV_REDUCE_SUM);
	
    //busca los inliers
    // a '1' los elementos <= que umbral, los inliers
    cvCmpS(dist1D, UMBRAL_DIST_UP*UMBRAL_DIST_UP, v_outliers, CV_CMP_LE);
    //se ponen a 0 (como outlier) los que estén a '0' en la máscara de entrada
    cvAnd(v_outliers, maskin, v_outliers);
    inliersint = cvCountNonZero(v_outliers);
    //se ponen a 1 los outliers ó 0 en maskin, y a 0 los inliers
    cvNot(v_outliers, v_outliers);
    //pone a zero en error en los outliers
    cvSet(dist1D, cvScalar(0), v_outliers);
    // suma del error de los inliers
    *errorint = cvSum(dist1D).val[0];
    return inliersint;
}


void CView2D::calcPose(CvMat *maskin )
{
    //if (maskin == NULL)
    //maskin = maskset;
    
    CvMat *points2;
    CvPOSITObject* object;
    int  npointsmask, i, j;
    CvPoint3D32f * datos3D;

    if (maskin)
    {
	npointsmask = cvCountNonZero(maskin);
    }
    else 
	npointsmask = npoints;
    
    points2 = cvCreateMat(npointsmask, 2, CV_32FC1);
    datos3D = (CvPoint3D32f *)malloc(npointsmask * sizeof(CvPoint3D32f));

    //selecciona los puntos a usar para el posit, según la máscara de entrada
    i=0, j=0;
    while (i<npointsmask)
    {
	if (maskin == NULL || cvGetReal1D(maskin,j))
	{
	    // A la vez, se desplazan los puntos al centro óptico de la imagen 
	    // restando (x,y) - (flx0,fly0) de cada punto. 
	    // flx0 y fly0 indican el centro óptico
	    cvmSet(points2, i, 0, cvmGet(points2d, j, 0) - flx0);
	    cvmSet(points2, i, 1, cvmGet(points2d, j, 1) - fly0);
	    datos3D[i].x = (float)cvmGet(p_linkmodel->points3d, j, 0);
	    datos3D[i].y = (float)cvmGet(p_linkmodel->points3d, j, 1);
	    datos3D[i].z = (float)cvmGet(p_linkmodel->points3d, j, 2);
	    i++;
	}
	j++;
    }
    
    //objeto posit
    object = cvCreatePOSITObject( datos3D, npointsmask );

    //la translación que se obtiene con el posit marca al punto 0 del subconjunto usado,
    // , no al punto 0 del modelo, si maskin (0,0) == 0
    //haabrá un error en la translación, que habrá que corregir más tarde

    //se llama a posic
    cvPOSIT( object, (CvPoint2D32f*) points2->data.fl, flFocalLength, criteria,
	     rotation->data.fl, translation->data.fl );

    //posit devuelve la rotación como matriz 3x3
    cvRodrigues2(rotation, vect);

    // ahora hay que corregir el error de translación, si lo hay
    if (maskin != NULL && cvGetReal1D(maskin,0) == 0)
    {	//el primer punto del modelo no es el que era, así que el vector de translación
	//ha cambiado. Ahora el primer punto es el datos3D[0];
	
	//vector de punto 0 original al actual punto 0: datos3D[0] - points3D[0];
	cvmSet(T_temp, 0, 0, - (datos3D[0].x - cvmGet(p_linkmodel->points3d, 0,0)) );
	cvmSet(T_temp, 1, 0, - (datos3D[0].y - cvmGet(p_linkmodel->points3d, 0,1)) );
	cvmSet(T_temp, 2, 0, - (datos3D[0].z - cvmGet(p_linkmodel->points3d, 0,2)) );

	cvMatMulAdd(rotation, T_temp, translation, translation);
    }

    cvReleasePOSITObject(&object);
    cvReleaseMat(&points2);
    free( datos3D );
}


void CView2D::reproject()
{
    cvProjectPoints2(p_linkmodel->points3d, vect, translation, calibration->KK, NULL, points2d);
}

void CView2D::reproject2()
{
    cvProjectPoints2(p_linkmodel->points3d, vect, translation, calibration->KK, NULL, points2d_reproject);
}


void CView2D::getNotHiden()
{
    //anulamos los puntos que se pasen del angulo correspondiente
    CView::getNotHiden(p_linkmodel->angY);
}


void CView2D::ChangeView(CView2D &view2)
{
    cvRodrigues2(view2.vect, view2.rotation);
    cvMatMul(calibration->R, view2.rotation, rotation);
    cvRodrigues2(rotation, vect);
    cvMatMulAdd(calibration->R, view2.translation, calibration->T, translation);
}    

void CView2D::set_points(int points) 
{ 
    npoints = points;

    //TODO: change size of all the matrixes
}       

void CView2D::set2z()
{
    cvmSet(translation, 0,0,0);
    cvmSet(translation, 1,0,0);
}

void CView2D::linkmodel(CModel3D &pmodel, CALIBRATION_PARAMS_t *calibration_params )
{
    p_linkmodel = &pmodel;
    calibration = calibration_params;

    criteria.type = CV_TERMCRIT_EPS|CV_TERMCRIT_ITER;

    criteria.epsilon = 0.0025f;
    criteria.max_iter = 2000;

    flFocalLength = (cvmGet(calibration->KK, 0,0) + cvmGet(calibration->KK, 1,1))/2;
    flx0 = cvmGet(calibration->KK, 0,2);
    fly0 = cvmGet(calibration->KK, 1,2);

}

void CView2D::alloc(int points)
{
    npoints = points;
    dealloc();
    m_alloc = true;
    maskset = cvCreateMat(npoints, 1, CV_8UC1);	  
    cvSet(maskset, cvScalar(1));
    CView::alloc(points);
    maskout = cvCloneMat(maskset);
    maskoutliers = cvCloneMat(maskset);
    maskout2d = cvCreateMat(npoints, 2, CV_8UC1);
    points2d_reproject =  cvCreateMat(npoints, 2, CV_32FC1); 
    pointstemp =  cvCreateMat(npoints, 2, CV_32FC1); 
    cvReshape(points2d, &points2d2, 2, 0);
    dist1D = cvCreateMat(npoints, 1, CV_32FC1);

    masktemp = cvCreateMat(npoints, 1, CV_8UC1);	    	    
}
	
void CView2D::dealloc()
{
    if (!m_alloc)
	return;
    m_alloc = false;
    cvReleaseMat(&mask_show);
    cvReleaseMat(&maskout2d);
    cvReleaseMat(&maskout);
    cvReleaseMat(&maskoutliers);
    cvReleaseMat(&points2d_reproject);
    cvReleaseMat(&pointstemp);
    cvReleaseMat(&masktemp);
    cvReleaseMat(&maskset);
}

int CView2D::ReadModelFromFile(const std::string filename, int n, CvRect roi)
{
    std::string name;
    CvFileStorage* fs = cvOpenFileStorage( filename.c_str(), 0, CV_STORAGE_READ );
    if (fs == NULL) //file is not valid
	return 0;

    if (points2d)
	cvReleaseMat(&points2d);

    if (n==0)
	name = "pointsI";
    else
	name = "pointsD";
	    
    points2d = (CvMat *) cvReadByName(fs, NULL, name.c_str());

    if (!points2d)
    {
	if (n==0)
	    name = "pointsL";
	else
	    name = "pointsR";

	points2d = (CvMat *) cvReadByName(fs, NULL, name.c_str());
    }
    cvReleaseFileStorage( &fs );

    alloc( points2d->rows);

    if (roi.x != 0 || roi.y != 0)
	cvSubS(&points2d2, cvScalar(roi.x, roi.y), &points2d2); 
    return npoints;
}

void CView2D::SaveModel(const std::string filename, int n, CvRect roi)
{
    CvFileStorage* fs = cvOpenFileStorage( filename.c_str(), 0, CV_STORAGE_WRITE );
    SaveModel(fs, n, roi);
    cvReleaseFileStorage(&fs);
}

void CView2D::SaveModel( CvFileStorage* fs, int n, CvRect roi)
{
    std::string name;
    if (n==0)
	name = "pointsL";
    else
	name = "pointsR";
	
    cvAddS(&points2d2, cvScalar(roi.x, roi.y), &points2d2);
    cvWrite(fs, name.c_str(), points2d);
    cvSubS(&points2d2, cvScalar(roi.x, roi.y), &points2d2);
}

    
int CView2D::closer_point(int x, int y)
{
    double min=600.0*600.0;
    int minpos=-1;
    double distx, disty;

    for (int i=0; i<npoints; i++)
    {
	distx = cvmGet(points2d, i,0)-x;
	disty = cvmGet(points2d, i,1)-y;
	distx = distx*distx + disty*disty;
	if (distx < min)
	{
	    min = distx;
	    minpos = i;
	}
    }
    return minpos;
	
}

CvMat * CView2D::Rneg()
{
    CvMat *minusvect = cvCloneMat(vect);
    cvScale(minusvect, minusvect, -1);
    cvRodrigues2(minusvect, rotation);
    //cvRodrigues2(vect, rotation);
    //cvScale(rotation, rotation, -1);
    return rotation;
}


int CView2D::addpoint()
{
    //change size of CView::mask
    CView::addpoint();
    
    //change size of matrices
    AddRow2new(&points2d);
    AddRow2new(&points2d_reproject);
    AddRow2new(&pointstemp);
    AddRow2new(&maskout);
    AddRow2new(&maskoutliers);
    AddRow2new(&maskout2d);
    AddRow2new(&maskset);
    AddRow2new(&masktemp);
    AddRow2new(&dist1D);
    cvReshape(points2d, &points2d2, 2, 0);

    //fill data to the new rows
    cvSet1D(masktemp, npoints, cvScalar(1));
    npoints ++;

    return npoints;
}    



int CView2D::deletepoint(int index)
{

    DeleteRow(&points2d, index );
    DeleteRow(&points2d_reproject, index );
    DeleteRow(&pointstemp, index );
    DeleteRow(&maskout, index);
    DeleteRow(&maskoutliers, index);
    CView::deletepoint(index);
    DeleteRow(&maskout2d, index);
    DeleteRow(&masktemp, index);
    DeleteRow(&maskset, index);
    cvReshape(points2d, &points2d2, 2, 0);
    DeleteRow(&dist1D, index );
    
    npoints --;
    return npoints;
}



void CView2D::reinit_SMAT(const char * mname, int size)
{
    //creación del modelo smat
    smat.reset();
    smat= load_SMAT_Model(mname, npoints, size);
    // desplazamos los puntos del modelo para que el SMAT use el punto en el centro, no en la esquina.
    if (points2d_T)
	cvReleaseMat(&points2d_T);
    points2d_T = cvCreateMat(2, npoints, CV_32FC1);


}

void CView2D::init_SMAT(CvSize sori, const char * mname, int size)
{
    size_smat_ori = cvSize(800, 800);
    factor = 1;
    size_smat_dest = cvSize(size_smat_ori.width/factor, size_smat_ori.height/factor);
    //size_smat_dest = cvSize(800, 0);
    //factor = size_smat_ori.width / size_smat_dest.width;
    //size_smat_dest.height = size_smat_ori.height/factor;

    smat_patch_size = size;

    img_smat     = cvCreateImage(size_smat_dest, IPL_DEPTH_32F, 1);
    imgtemp_smat = cvCreateImage(size_smat_dest, IPL_DEPTH_8U, 1);

    cvZero(img_smat);
    cvZero(imgtemp_smat);

    int x_ori = (sori.width-size_smat_ori.width);
    int y_dest = (size_smat_ori.height-sori.height);
    roiori = cvRect(x_ori/2, 0, size_smat_ori.width, sori.height);
    CvRect roidest2 = cvRect(0,y_dest/2, roiori.width, roiori.height);
    desplazar_parche = cvScalar( smat_patch_size/2*factor + roiori.x , 
				 smat_patch_size/2*factor - roidest2.y, 0, 0);
    roidest =cvRect(roidest2.x/factor, roidest2.y/factor, roidest2.width/factor, roidest2.height/factor);
    
    points2d_T=0;
    reinit_SMAT(mname, smat_patch_size);
}


void CView2D::preprocess_smat_image(IplImage *img)
{
    cvSetImageROI(img, roiori);
    cvSetImageROI(img_smat, roidest);
    cvSetImageROI(imgtemp_smat, roidest); //already done
    
    //TODO: join the cvConvert and cvResize in only one function
    //cvConvert(img, imgtemp); //converts from 8U to 32F
    //cvResize(imgtemp, img_smat);// changes image size. 32F both
    cvResize(img, imgtemp_smat); //changes size. 8U type
    cvConvert(imgtemp_smat, img_smat); //converts from 8U to 32F.

    cvResetImageROI(img);
    cvResetImageROI(img_smat);
    cvResetImageROI(imgtemp_smat);

}


int CView2D::SMAT ()
{ 	   	
    //anulamos los puntos que se pasen del angulo correspondiente
    getNotHiden();

    // Desplazamos los puntos del modelo para que el SMAT use el punto en el centro,
    // no en la esquina.
    cvSubS(&points2d2, desplazar_parche, &points2d2);

    /****** SMAT *****/
    cvTranspose(points2d, points2d_T);

    //set the points for the left/right image
    //cvScale( points2d_T, points2d_T, 0.5);
    smat->shape->set_shape(points2d_T);
    smat->track(img_smat, cvPoint(0,0), mask_show);
    points2d_T = smat->shape->get_shape().get();
    //cvScale(points2d_T, points2d_T,  2);

    cvTranspose(points2d_T, points2d);
	
    // REdesplazamos los puntos del modelo 
    // para que el SMAT use el punto en el centro,no en la esquina.
    cvAddS(&points2d2, desplazar_parche, &points2d2);

    return 1;
}


void CView2D::display_SMATModel(IplImage *img)
{
    ::display_SMATModel(img, imgtemp_smat, smat);
}

CvPoint CView2D::Point(int i)
{
    return cvPoint(cvmGet(points2d,i,0), cvmGet(points2d,i,1));
}
